package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class FinalBot extends SimpleRobot {
    Forklift forklift;
    Deployment minibot;
    SignalLED led;
    RobotDrive drive;
    Joystick driveStick;
    ForkliftStick forkliftStick;
    DriverStation ds;
    LineTracker tracker;
    boolean driveInverted = false;

    float forkUpTime = 0.5f;
    float forkDownTime = 0.5f;
    float straightTime = 2.0f;

    public FinalBot(){
        forklift = new Forklift();
        minibot = new Deployment();
        led = new SignalLED();
        //swap left and right motors
        drive = new RobotDrive(1, 2);
        driveStick = new Joystick(2);
        forkliftStick = new ForkliftStick();
        ds = DriverStation.getInstance();
        tracker = new LineTracker(drive);

        // init the watchdog
        getWatchdog().setExpiration(0.1);
    }

    public boolean driveEnabled()
    {
        return ds.getDigitalIn(3);
    }
    public boolean forkLiftEnabled()
    {
        return ds.getDigitalIn(4);
    }
    public boolean deployEnabled()
    {
        return ds.getDigitalIn(5);
    }
    public boolean signalLEDEnabled()
    {
        return ds.getDigitalIn(6);
    }
    public boolean getDriveMode()
    {
        return !ds.getDigitalIn(7);
    }
    public boolean lineFollowingEnabled()
    {
        return ds.getDigitalIn(8);
    }
    public boolean autonForkliftEnabled()
    {
        return forkLiftEnabled();
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        //init timer
        Timer timer = new Timer();
        timer.start();
        timer.reset();

        //disable watchdog
        getWatchdog().setEnabled(false);

        System.out.println("Starting autonomous");

        //setup line folowing
        if(lineFollowingEnabled()){
            //setup line tracker
            System.out.println("Setting up Line Tracker");

            //set power profile and turn gain for line following from driver station
            tracker.setDefaultSteeringGain(ds.getAnalogIn(4) / 2.5 - 1.0);
            double ppSelect = ds.getAnalogIn(1);
            if(ppSelect < 1.0){
                tracker.forkProfile =     new double[]{1.0, 1.0, 0.7, 0.8, 0.8, 0.8, 0.8, 0.7, 0.7};
                tracker.straightProfile = new double[]{1.0, 1.0, 0.7, 0.8, 0.8, 0.8, 0.8, 0.7, 0.7};
            }
            else if(ppSelect < 2.0)
            {
                tracker.straightProfile = new double[]{1.0, 1.0, 0.6, 0.7, 0.7, 0.7, 0.7, 0.6, 0.6};
                tracker.forkProfile =     new double[]{1.0, 1.0, 0.6, 0.7, 0.7, 0.7, 0.7, 0.6, 0.6};
            }
            else if(ppSelect < 3.0){
                tracker.straightProfile = new double[]{0.9, 0.9, 0.6, 0.7, 0.7, 0.7, 0.7, 0.6, 0.6};
                tracker.forkProfile =     new double[]{0.9, 0.9, 0.6, 0.7, 0.7, 0.7, 0.7, 0.6, 0.6};
            }
            else if(ppSelect < 4.0){
                tracker.straightProfile = new double[]{0.7, 0.8, 0.7, 0.6, 0.6, 0.6, 0.6, 0.4};
                tracker.forkProfile =     new double[]{0.7, 0.8, 0.7, 0.6, 0.6, 0.6, 0.6, 0.4};
            }
            else if(ppSelect < 5.0){
                tracker.straightProfile = new double[]{1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
                tracker.forkProfile =     new double[]{1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
            }

            tracker.reset();
        }
        else{
            straightTime = (float) ds.getAnalogIn(4) * 3;
        }
        //setup forklift
        if(autonForkliftEnabled()){
            forkUpTime = (float) ds.getAnalogIn(1);
            forklift.setTrim(0.2);
        }

        while(isAutonomous() && isEnabled()){
            double currentTime = timer.get();
            
            // move forklift up for the specified ammount of time
            // this happens at the same time the robot is following the line
            if(autonForkliftEnabled()){
                if(currentTime < forkUpTime){
                    forklift.setVelocity(0.7);
                }
                else{
                    forklift.setVelocity(0.0);
                }
            }
                
            if(lineFollowingEnabled()){
                if(!tracker.finished()){
                    // first, follow the line
                    tracker.followLine();
                }
                else if(autonForkliftEnabled()){
                    // then, lower the forklift a little
                    forklift.setVelocity(-0.3);  // move forklift down
                    Timer.delay(forkDownTime);   // wait
                    forklift.setVelocity(0);     //stop forklift
                }
            }
            else{
                if(currentTime < straightTime){
                    drive.arcadeDrive(0.7, -0.0);
                }
                else if(autonForkliftEnabled()){
                    drive.arcadeDrive(0,0);
                    // then, lower the forklift a little
                    forklift.setVelocity(-0.3);  // move forklift down
                    Timer.delay(forkDownTime);   // wait
                    forklift.setVelocity(0);     //stop forklift
                }
            }

            Timer.delay(0.01);
        }

        System.out.println("Autonomous Finished");
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        // enable the watchdog
        getWatchdog().setEnabled(true);
        int iteration = 0;

        while(isOperatorControl()){
            // pet the watchdog
            getWatchdog().feed();

            if(driveEnabled()){
                if(driveStick.getRawButton(1)){
                    driveInverted = false;
                }
                else if(driveStick.getRawButton(3)){
                    driveInverted = true;
                }
                double maxPower = 1.0;
                if(!driveStick.getRawButton(8)){
                    maxPower = ds.getAnalogIn(3) / 5.0;
                }
                if(!getDriveMode()){
                    double rotationTrim = ds.getAnalogIn(2) / 2.5 - 1.0;
                    double rotationValue = Math.max(-1.0, Math.min(driveStick.getRawAxis(1) + rotationTrim, 1.0));
                    if(!driveInverted){
                        drive.arcadeDrive(-driveStick.getRawAxis(2) * maxPower, rotationValue * maxPower, lineFollowingEnabled());
                    }
                    else{
                        drive.arcadeDrive(driveStick.getRawAxis(2) * maxPower, -rotationValue * maxPower, lineFollowingEnabled());
                    }
                }
                else{
                    if(!driveInverted){
                        drive.tankDrive(-driveStick.getRawAxis(4) * maxPower, -driveStick.getRawAxis(2) * maxPower);
                    }
                    else{
                        drive.tankDrive(driveStick.getRawAxis(2) * maxPower, driveStick.getRawAxis(4) * maxPower);
                    }
                }
            }
            if(forkLiftEnabled()){
                forklift.setTrim(forkliftStick.getTrim());
                forklift.setVelocity(forkliftStick.getVelocity());
            }
            if(deployEnabled()){
                if(forkliftStick.getRawButton(8) || forkliftStick.getRawButton(9)){
                    System.out.print("DEPLOYING");
                    minibot.deploy();
                }
                else if(forkliftStick.getRawButton(10) || forkliftStick.getRawButton(11))
                {
                    minibot.stop();
                }
                else if(forkliftStick.getRawButton(6) || forkliftStick.getRawButton(7))
                {
                    minibot.retract();
                }
                else if(minibot.isRetracting()){
                    minibot.stop();
                }
                minibot.updateVelocity();
                ds.setDigitalOut(1, minibot.isDeployed());
            }
            if(signalLEDEnabled()){
                if(forkliftStick.getRawButton(4)){
                    led.setColor(SignalLED.RED);
                }
                else if(forkliftStick.getRawButton(3)){
                    led.setColor(SignalLED.WHITE);
                }
                else if(forkliftStick.getRawButton(5)){
                    led.setColor(SignalLED.BLUE);
                }
                else if(forkliftStick.getRawButton(2)){
                    led.setColor(SignalLED.OFF);
                }
            }

            // sleep so threads can process
            Timer.delay(0.005);
            iteration++;
        }

        // TODO: shut off all motors
    }
}
